import lejos.nxt.Button;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.addon.CompassSensor;
import lejos.nxt.comm.RConsole;
import lejos.robotics.navigation.CompassPilot;

public class Exercise1 {

	// 3.6f is actual measurement
	// but 3.25 was more
	// accurate
	private static final float WHEEL_DIA = 3.25f;
	private static final float TRACK_WIDTH = 13f;
	private static final Motor LEFT_MOTOR = Motor.C;
	private static final Motor RIGHT_MOTOR = Motor.A;
	private static final int NUM_RUNS = 5;

	public static void main(String[] args) {
		RConsole.openBluetooth(0);
		Problem1();
		// Problem2();
		// Problem3();
		RConsole.close();
	}

	private static void Problem1() {
		System.out.println("EXECUTING PROBLEM 1");
		System.out.println("CALIBRATE");
		Button.waitForPress();

		CompassPilot pilot = new CompassPilot(new CompassSensor(SensorPort.S3),
				WHEEL_DIA, TRACK_WIDTH, LEFT_MOTOR, RIGHT_MOTOR);
		pilot.calibrate();

		for (int i = 0; i < NUM_RUNS; ++i) {
			System.out.println("TRAVEL 3 ft");
			Button.waitForPress();
			pilot.resetCartesianZero();
			RConsole.println("ANGLE: " + pilot.getAngle());
			System.out.println("ANGLE: " + pilot.getAngle());
			pilot.setHeading((int) pilot.getAngle());
			pilot.travel(inToCm(36));
			RConsole.println("DISTANCE: " + cmToIn(pilot.getTravelDistance())
					+ "\nANGLE: " + pilot.getAngle());
			System.out.println("DISTANCE: " + cmToIn(pilot.getTravelDistance())
					+ "\nANGLE: " + pilot.getAngle());
		}

		System.out.println("PRESS ANY BUTTON");
		Button.waitForPress();
	}

	private static void Problem2() {
		System.out.println("EXECUTING PROBLEM 2");
		System.out.println("CALIBRATE");
		Button.waitForPress();

		CompassPilot pilot = new CompassPilot(new CompassSensor(SensorPort.S3),
				WHEEL_DIA, TRACK_WIDTH, LEFT_MOTOR, RIGHT_MOTOR);
		pilot.calibrate();

		for (int i = 0; i < NUM_RUNS; ++i) {
			System.out.println("ROTATE 90");
			Button.waitForPress();
			pilot.resetCartesianZero();
			pilot.rotate(90);
			RConsole.println("ANGLE: " + pilot.getAngle());
			System.out.println("ANGLE: " + pilot.getAngle());

			System.out.println("ROTATE 180");
			Button.waitForPress();
			pilot.resetCartesianZero();
			pilot.rotate(180);
			RConsole.println("ANGLE: " + pilot.getAngle());
			System.out.println("ANGLE: " + pilot.getAngle());
		}

		System.out.println("PRESS ANY BUTTON");
		Button.waitForPress();
	}

	private static void Problem3() {
		System.out.println("EXECUTING PROBLEM 3");
		System.out.println("CALIBRATE");
		Button.waitForPress();

		UltrasonicSensor sonic = new UltrasonicSensor(SensorPort.S2);
		LightSensor light = new LightSensor(SensorPort.S4);
		CompassPilot pilot = new CompassPilot(new CompassSensor(SensorPort.S3),
				WHEEL_DIA, TRACK_WIDTH, LEFT_MOTOR, RIGHT_MOTOR);
		pilot.calibrate();

		for (int i = 0; i < NUM_RUNS; ++i) {
			System.out.println("CALC DIST");
			Button.waitForPress();
			RConsole.println("DIST " + sonic.getDistance() + " "
					+ sonic.getUnits());
			System.out.println("DIST " + sonic.getDistance() + " "
					+ sonic.getUnits());

			for (int j = 0; j < 5; ++j) {
				System.out.println("RESET ZERO");
				Button.waitForPress();
				pilot.resetCartesianZero();
				System.out.println("ROTATE " + 90 * j);
				Button.waitForPress();
				RConsole.println("ANGLE " + pilot.getAngle());
				System.out.println("ANGLE " + pilot.getAngle());
			}

			System.out.println("CALC DARK");
			Button.waitForPress();
			RConsole.println("DARK " + light.readNormalizedValue());
			System.out.println("DARK " + light.readNormalizedValue());

			System.out.println("CALC LIGHT");
			Button.waitForPress();
			RConsole.println("LIGHT " + light.readNormalizedValue());
			System.out.println("LIGHT " + light.readNormalizedValue());
		}
		System.out.println("PRESS ANY BUTTON");
		Button.waitForPress();
	}

	private static float inToCm(float in) {
		return in * 2.54f;
	}

	private static float cmToIn(float cm) {
		return cm / 2.54f;
	}
}
